/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : sensors.c
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "main.h"


int16_t sensor_data_int[10]    = {0};      // data from sensors.  3 int from accel(0..2) + 3 int from gyro(3..5)
int16_t sensor_bias[10]        = {0};      // sensor biases
int32_t sensor_mas[10]         = {0};      // sensor moving average star MA*[i]


/*************************************************************************
 read, calibrate, normalize, LP filter  data from sensors
*************************************************************************/
void populateSensorData()
{
    //read XYZ data from L3G4200D - 612us
    // read from register 0x28 + MSb set to auto increment
    i2cRead(L3G4200D_ADDR,((0x28) | (1<<7)),6,(uint8_t *)&sensor_data_int[SEN_GX]);

    //read accel and mag from LSM303DLHC


    //normalize accel and mag?

    //Moving average filter
    //http://www.daycounter.com/LabBook/Moving-Average.phtml
    //MA*[i]= MA*[i-1] +X[i] - MA*[i-1]/N
    //where MA* is the moving average*N.
    //MA[i]= MA*[i]/N
    for(uint8_t j=0; j<9; j++) //60.4us
    {
        sensor_data_int[j] <<= NPOINT_AVERAGE; //increase accuracy
        sensor_mas[j] += sensor_data_int[j] - (sensor_mas[j] >> NPOINT_AVERAGE);// /NPOINT_AVERAGE=2 -> 2^2 - averaging over 4 points;
        sensor_data_int[j] = (sensor_mas[j] >> (2*NPOINT_AVERAGE)); // / (NPOINT_AVERAGE*NPOINT_FILTER_ACCURACY);
    }

    for(uint8_t j=0; j<9; j++)  //10us
        sensor_data_int[j] -= sensor_bias[j];
}


/*************************************************************************
 Calibrate Gyro
*************************************************************************/
void calibrateGyro()
{
    int32_t accumulator[3] = {0};
    uint16_t counter;

    for (counter=0; counter<(CALIB_SAMPLES*4); counter++) //first loop - read samples to make sure reading is up to date
    {
        LED_GREEN_FLIP;  // flash GREEN led to signify initialization
        populateSensorData();
        delay(25);
    }

    for (counter=0; counter<CALIB_SAMPLES; counter++)
    {
        LED_RED_FLIP;  // flash red led to signify calibration
        populateSensorData();

        for(uint8_t i=0; i<3; i++)
            accumulator[i] += sensor_data_int[i+SEN_GX];

        //while (cg25ms != g25msec);  // wait for next 100ms period - one sample every 100ms
        delay(100);
    }

    if(counter>0)
    {
        for(uint8_t i=0; i<3; i++)
            sensor_bias[i+SEN_GX] = accumulator[i]/counter;
    }
}

/*************************************************************************
 Calibrate Compass
*************************************************************************/
void calibrateCompassAccel()
{
    // measure min and max for each axis until interrupted
    // when done
    // bias[axis] = (min+max)/2
    int16_t min[6] = {0};
    int16_t max[6] = {0};
    uint16_t sampleCount = 0;

    LED_RED_OFF;
    LED_GREEN_ON;
    while (sampleCount < 200) // 20 seconds of calibration
    {
        LED_RED_FLIP;  // flash red led to signify calibration
        LED_GREEN_FLIP;  // flash red led to signify calibration
        populateSensorData();

        for(uint8_t i=0; i<6; i++)
        {
            if(min[i]>sensor_data_int[i+SEN_AX]) min[i]=sensor_data_int[i+SEN_AX];
            if(max[i]<sensor_data_int[i+SEN_AX]) max[i]=sensor_data_int[i+SEN_AX];
        }

        delay(100);  // wait for next 100ms period - one sample every 100ms
        sampleCount++;
    }

    //once done - calculate biases and save to EEPROM
    for(uint8_t i=0; i<6; i++)
        sensor_bias[i+SEN_AX] = (min[i] + max[i]) / 2;

    //Save to EEPROM
}


/*************************************************************************
 set up LSM303DLHC
*************************************************************************/
uint8_t lsm303dlhc_init()
{
    bool ack = false;
    uint8_t sig = 0;


    ack = i2cRead(LSM303DLHC_ACCEL_ADDR, 0x27, 1, &sig);
    if (!ack)
        return 0;

    //ACCEL
    //CTRL_REG1_A (20h) = 0x77
    //400Hz, Normal mode, X enable, Y enable, Z enable
    i2cWrite(LSM303DLHC_ACCEL_ADDR, 0x20, 0x77);

    //CTRL_REG4_A (23h) = 0x38
    //BDU=0, BLE=0, FS1/0=11 (+-16G), HR=1, SIM=0
    i2cWrite(LSM303DLHC_ACCEL_ADDR, 0x23, 0x38);

    //MAG
    //CRA_REG_M (00h) = 0x1C
    //Temp=0, D02-D00 = 111 (220Hz)
    i2cWrite(LSM303DLHC_MAG_ADDR, 0x00, 0x1C);

    //CRB_REG_M (01h) = 0x20
    //GAIN = 1.3Gauss
    i2cWrite(LSM303DLHC_MAG_ADDR, 0x01, 0x20);

    //MR_REG_M (02h) = 0x00
    //Continuous conversion
    i2cWrite(LSM303DLHC_MAG_ADDR, 0x02, 0x00);

    return 1;
}

/*************************************************************************
 set up L3G4200D
*************************************************************************/
uint8_t l3g4200d_init()
{
    bool ack = false;
    uint8_t sig = 0;

    ack = i2cRead(L3G4200D_ADDR, 0x0F, 1, &sig);
    if (!ack || sig != L3G4200D_ID)
        return 0;


    i2cWrite(L3G4200D_ADDR, 0x21, 0x34); //CTRL_REG2
    i2cWrite(L3G4200D_ADDR, 0x23, 0xA0); //CTRL_REG4 -
    //BDU = 1: output registers not updated until MSB and LSB reading
    //BLE = 0: Data LSB
    //FS1-FS0 = 10: 2000 dps
    i2cWrite(L3G4200D_ADDR, 0x24, 0x13); //CTRL_REG5
    i2cWrite(L3G4200D_ADDR, 0x20, 0xFF); //CTRL_REG1

    return 1;
}




float l3g4200dToRPS(int16_t x)
{
    return ((float)x)*L3G4200D_DPS_PER_DIGIT*M_PI/180.0f;
}


